#include <string.h>
#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include "sound.h"
#include "data.h"

#define TRUE 1
#define FALSE 0

#define PIf 3.14159265359f

static int sound_background(Sound* sound);
static int sound_frightened(Sound* sound);
static int sound_eat1(Sound* sound);
static int sound_eat2(Sound* sound);
static int sound_eat_ghost(Sound* sound);
static int sound_eat_fruit(Sound* sound);
static int sound_ghost_return(Sound* sound);

static float clampf(float v, float min, float max) {
    if (v < min) return min;
    if (v > max) return max;
    return v;
}

static void set_filter_params(LowPassFilter* filter, unsigned int sample_rate, float f0, float q) {
    float w0 = 2.0f * PIf * f0 / sample_rate;
    float cos = cosf(w0);
    float sin = sinf(w0);
    float alpha = sin / (2 * q);

    float a0 = 1.0f + alpha;

    filter->a[1] = -2.0f * cos;
    filter->a[2] = 1.0f - alpha;

    filter->b[0] = (1.0f - cos) / 2.0f;
    filter->b[1] = 1.0f - cos;
    filter->b[2] = (1.0f - cos) / 2.0f;
    
    filter->a[1] /= a0;
    filter->a[2] /= a0;
    filter->b[0] /= a0;
    filter->b[1] /= a0;
    filter->b[2] /= a0;
}

static float filter_process(LowPassFilter* filter, float sample) {
    filter->x[2] = filter->x[1];
    filter->x[1] = filter->x[0];
    filter->y[2] = filter->y[1];
    filter->y[1] = filter->y[0];

    filter->x[0] = sample;
    filter->y[0] = filter->b[0] * filter->x[0] + filter->b[1] * filter->x[1] + filter->b[2] * filter->x[2]
                                               - filter->a[1] * filter->y[1] - filter->a[2] * filter->y[2];

    return filter->y[0];
}

void sound_init(SoundManager* sound) {
    memset(sound, 0, sizeof(SoundManager));
    sound->audio_buffer = malloc(AUDIO_BUFFER_SIZE * sizeof(float));

    for (int i = 0; i < NUM_VOICES; i++) {
        sound->voices[i].volume = 0.5f;
    }
}

void sound_free(SoundManager* sound) {
    free(sound->audio_buffer);
}

static void update_sounds(SoundManager* mgr) {
    for (int i = 0; i < MAX_SOUNDS; i++) {
        Sound* sound = &mgr->sounds[i];
        if (!sound->active) continue;

        int ret = -1;
        switch (sound->type) {
            case SOUND_BACKGROUND: ret = sound_background(sound); break;
            case SOUND_FRIGHTENED: ret = sound_frightened(sound); break;
            case SOUND_EAT1: ret = sound_eat1(sound); break;
            case SOUND_EAT2: ret = sound_eat2(sound); break;
            case SOUND_EAT_GHOST: ret = sound_eat_ghost(sound); break;
            case SOUND_EAT_FRUIT: ret = sound_eat_fruit(sound); break;
            case SOUND_GHOST_RETURN: ret = sound_ghost_return(sound); break;
        }
        assert(ret != -1);

        mgr->voices[sound->voice_index].volume = (float)sound->volume / 16.0f;
        mgr->voices[sound->voice_index].frequency = sound->frequency;
        mgr->voices[sound->voice_index].waveform = sound->waveform;
        sound->tick++;

        if (ret) {
            mgr->voices[sound->voice_index].volume = 0;
            sound->active = FALSE;
        }
    }
}

static void update_dump(SoundManager* mgr) {
    for (int i = 0; i < NUM_VOICES; i++) {
        mgr->voices[i].volume = 0;
    }

    if (mgr->dump_index >= mgr->dump_size) {
        mgr->current_dump = NULL;
        return;
    }

    for (int i = 0; i < mgr->dump_voices; i++) {
        uint32_t data = mgr->current_dump[mgr->dump_index++];

        mgr->voices[i].frequency = data & 0xfffff; // 20 bits frequency
        mgr->voices[i].waveform = (data >> 24) & 0x7; // 3 bits waveform
        mgr->voices[i].volume = ((data >> 28) & 0xF) / 15.0f; // 4 bits volume
    }
}

void sound_update(SoundManager* mgr) {
    float filter_freq = 35000.0f;
    float max_filter_freq = (float)mgr->sample_rate * 0.4f;
    if (filter_freq > max_filter_freq) filter_freq = max_filter_freq;

    for (int i = 0; i < NUM_VOICES; i++) {
        set_filter_params(&mgr->filter[i], AUDIO_CLOCK_RATE, filter_freq, 0.21f);
    }

    // update sounds
    if (mgr->current_dump) {
        update_dump(mgr);
    } else {
        update_sounds(mgr);
    }

    // update voices
    const unsigned int sample_len_ns = 1000000000 / mgr->sample_rate;
    const unsigned int clock_len_ns = 1000000000 / AUDIO_CLOCK_RATE;
    mgr->time_accum += (unsigned int)((1.0f / 60.0f) * 1000000000);

    assert(mgr->submit_audio != NULL);
    while (mgr->time_accum >= sample_len_ns) {
        mgr->clock_time_accum += sample_len_ns;

        while (mgr->clock_time_accum >= clock_len_ns) {
            for (int v = 0; v < NUM_VOICES; v++) {
                Voice* voice = &mgr->voices[v];
                unsigned int f = voice->frequency < 0 ? 0 : voice->frequency;
                voice->counter = (voice->counter + f) & 0xfffff;

                unsigned int sample = WAVEFORM_DATA[((voice->counter >> 15) % 32) + voice->waveform * 32];
                voice->sample = (float)sample / 15.0f * 2.0f - 1.0f;
                voice->sample = filter_process(&mgr->filter[v], voice->sample);
            }

            mgr->clock_time_accum -= clock_len_ns;
        }

        float mix_sample = 0.0f;

        for (int v = 0; v < NUM_VOICES; v++) {
            Voice* voice = &mgr->voices[v];
            mix_sample += voice->sample * voice->volume * (1.0f / NUM_VOICES);
        }

        // write sample
        mgr->audio_buffer[mgr->buffer_index++] = clampf(mix_sample, -1.0f, 1.0f);

        // submit audio data when finished
        if (mgr->buffer_index >= AUDIO_BUFFER_SIZE) {
            if (mgr->submit_audio != NULL)
                mgr->submit_audio(mgr->audio_buffer, AUDIO_BUFFER_SIZE, mgr->audio_userdata);
            
            mgr->buffer_index = 0;
        }

        mgr->time_accum -= sample_len_ns;
    }
}

void sound_start(SoundManager* mgr, SoundType sound_type) {
    for (int i = 0; i < MAX_SOUNDS; i++) {
        Sound* sound = &mgr->sounds[i];
        if (!sound->active) {
            memset(sound, 0, sizeof(Sound));
            sound->active = TRUE;
            sound->type = sound_type;
            break;
        }
    }
}

void sound_stop(SoundManager* mgr, SoundType sound_type) {
    for (int i = 0; i < MAX_SOUNDS; i++) {
        Sound* sound = &mgr->sounds[i];
        if (sound->type == sound_type) {
            sound->active = FALSE;
            break;
        }
    }
}

void sound_stop_all(SoundManager* mgr) {
    memset(mgr->sounds, 0, sizeof(mgr->sounds));

    for (int i = 0; i < NUM_VOICES; i++) {
        mgr->voices[i].volume = 0;
        mgr->voices[i].frequency = 0;
        mgr->voices[i].counter = 0;
    }
}

void sound_start_dump(SoundManager* mgr, uint8_t voices, const uint32_t* dump, size_t size) {
    mgr->current_dump = dump;
    mgr->dump_size = size;
    mgr->dump_index = 0;
    mgr->dump_voices = voices;
}

/////////////////////
// Sound functions //
/////////////////////

static int sound_background(Sound* sound) {
    sound->voice_index = 1;

    if (sound->tick == 0) {
        sound->volume = 6;
        sound->waveform = 6;
        sound->frequency = 0x1000;
    }

    if ((sound->tick % 24) < 12) {
        sound->frequency += 0x200;
    } else {
        sound->frequency -= 0x200;
    }

    return FALSE;
}

static int sound_frightened(Sound* sound) {
    sound->voice_index = 1;

    if (sound->tick == 0) {
        sound->volume = 10;
        sound->waveform = 4;
        sound->frequency = 0x180;
    } else if ((sound->tick % 8) == 0) {
        sound->frequency = 0x180;
    } else {
        sound->frequency += 0x180;
    }

    return FALSE;
}

static int sound_eat1(Sound* sound) {
    sound->voice_index = 2;

    if (sound->tick == 0) {
        sound->volume = 12;
        sound->waveform = 2;
        sound->frequency = 0x1500;
    } else if (sound->tick == 5) {
        sound->volume = 0;
        return TRUE;
    } else {
        sound->frequency -= 0x300;
    }

    return FALSE;
}

static int sound_eat2(Sound* sound) {
    sound->voice_index = 2;

    if (sound->tick == 0) {
        sound->volume = 12;
        sound->waveform = 2;
        sound->frequency = 0x0700;
    } else if (sound->tick == 5) {
        sound->frequency = 0;
        return TRUE;
    } else {
        sound->frequency += 0x0300;;
    }

    return FALSE;
}

static int sound_eat_ghost(Sound* sound) {
    sound->voice_index = 2;

    if (sound->tick == 0) {
        sound->volume = 12;
        sound->waveform = 5;
        sound->frequency = 0;
    } else if (sound->tick == 32) {
        sound->volume = 0;
        return TRUE;
    } else {
        sound->frequency += 0x20;
    }

    return FALSE;
}

static int sound_eat_fruit(Sound* sound) {
    sound->voice_index = 2;

    if (sound->tick == 0) {
        sound->volume = 15;
        sound->waveform = 6;
        sound->frequency = 0x1600;
    } else if (sound->tick == 23) {
        return TRUE;
    } else if (sound->tick < 11) {
        sound->frequency -= 0x200;
    } else {
        sound->frequency += 0x200;
    }

    return FALSE;
}

static int sound_ghost_return(Sound* sound) {
    sound->voice_index = 1;

    if (sound->tick == 0) {
        sound->volume = 12;
        sound->waveform = 0;
    }

    if ((sound->tick % 15) == 0) {
        sound->frequency = 0x6900;
    } else {
        sound->frequency -= 0x660;
    }

    return FALSE;
}